/*
 * @Description: odometry 信息发布
 * @Author: Zhijian Qiao
 * @Date: 2020-02-06 21:05:47
 */
#ifndef avp_slam_PUBLISHER_ODOMETRY_PUBLISHER_HPP_
#define avp_slam_PUBLISHER_ODOMETRY_PUBLISHER_HPP_

#include <string>

#include <Eigen/Dense>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>

namespace avp_slam {
    class OdometryPublisher {
    public:
        OdometryPublisher(ros::NodeHandle &nh,
                          std::string topic_name,
                          std::string base_frame_id,
                          std::string child_frame_id, //twist所在地的坐标系
                          int buff_size);

        OdometryPublisher() = default;

        void Publish(const Eigen::Matrix4f &transform_matrix, double time);

        void Publish(const Eigen::Matrix4f &transform_matrix);

        void Publish(const Eigen::Matrix4f &transform_matrix, double time, double start, double end);

        bool HasSubscribers();

    private:
        void PublishData(const Eigen::Matrix4f &transform_matrix, ros::Time time);

    private:
        ros::NodeHandle nh_;
        ros::Publisher publisher_;

        nav_msgs::Odometry odometry_;
        nav_msgs::Odometry odometry_last_;

        ros::Time time_last_;

        double roll_last, pitch_last, yaw_last;

        bool first = true;
    };
}
#endif